/*  
  需求：编写动作客户端实现，可以提交circle / rectangle类型到服务端，并处理服务端的连续反馈以及最终返回结果。
  步骤：
    1.包含头文件；
    2.初始化 ROS2 客户端；
    3.定义节点类；
      3-1.创建动作客户端；
      3-2.发送请求；
      3-3.处理目标发送后的反馈；
      3-4.处理连续反馈；
      3-5.处理最终响应。
    4.调用spin函数，并传入节点对象指针；
    5.释放资源。
*/
// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "auto_aim_interfaces/action/calib.hpp"

using auto_aim_interfaces::action::Calib;
using GoalHandleCalib = rclcpp_action::ClientGoalHandle<Calib>;
using namespace std::placeholders;

class CalibrationActionClient : public rclcpp::Node
{
public:
  explicit CalibrationActionClient(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions())
  : Node("calibration_action_client", node_options)
  {
    this->client_ptr_ = rclcpp_action::create_client<Calib>(this, "calibrate_camera");
  }

  void send_goal()
  {
    if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(10))) {
      RCLCPP_ERROR(this->get_logger(), "未能连接到动作服务器！");
      return;
    }

    auto goal_msg = Calib::Goal();
    goal_msg.calibration_type = "circle"; // 设置标定类型为 "circle"

    RCLCPP_INFO(this->get_logger(), "向动作服务器发送 'circle' 类型的标定请求...");

    auto send_goal_options = rclcpp_action::Client<Calib>::SendGoalOptions();
    send_goal_options.goal_response_callback =
      std::bind(&CalibrationActionClient::goal_response_callback, this, _1);
    send_goal_options.feedback_callback =
      std::bind(&CalibrationActionClient::feedback_callback, this, _1, _2);
    send_goal_options.result_callback =
      std::bind(&CalibrationActionClient::result_callback, this, _1);

    auto goal_handle_future = this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
  }

private:
  rclcpp_action::Client<Calib>::SharedPtr client_ptr_;

  void goal_response_callback(GoalHandleCalib::SharedPtr goal_handle)
  {
    if (!goal_handle) {
      RCLCPP_ERROR(this->get_logger(), "标定请求被服务器拒绝！");
    } else {
      RCLCPP_INFO(this->get_logger(), "标定请求被服务器接收，正在执行...");
    }
  }

  void feedback_callback(GoalHandleCalib::SharedPtr goal_handle, const std::shared_ptr<const Calib::Feedback> feedback)
  {
    RCLCPP_INFO(this->get_logger(), "正在标定...已拍摄 %d 张照片", feedback->photo_taken);
    RCLCPP_INFO(this->get_logger(), "正在标定...已处理 %d 张照片", feedback->photo_num);
  }

  void result_callback(const GoalHandleCalib::WrappedResult & result)
  {
    switch (result.code) {
      case rclcpp_action::ResultCode::SUCCEEDED:
        RCLCPP_INFO(this->get_logger(), "标定成功！");
        break;
      case rclcpp_action::ResultCode::ABORTED:
        RCLCPP_ERROR(this->get_logger(), "标定被中止");
        return;
      case rclcpp_action::ResultCode::CANCELED:
        RCLCPP_ERROR(this->get_logger(), "标定被取消");
        return;
      default:
        RCLCPP_ERROR(this->get_logger(), "标定失败，未知错误");
        return;
    }

    // 打印相机内参、畸变系数和误差率
    RCLCPP_INFO(this->get_logger(), "误差率: %lf", result.result->error);
  }
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  auto action_client = std::make_shared<CalibrationActionClient>();
  action_client->send_goal();
  rclcpp::spin(action_client);
  rclcpp::shutdown();
  return 0;
}
